﻿#pragma execution_character_set("utf-8")
#include "udpdata.h"


udpdata::udpdata() {

}

FlightData udpdata::parseFlightData(const QByteArray &data) {
    FlightData flightData;
    if (data.size() != udpdata::datasize) {
        qWarning() << QString("输入数据长度不对,收到了%1,规定的长度为：%2")
                          .arg(data.size())
                          .arg(udpdata::datasize);
        return flightData;
    }

    // Checksum,校验位在34位
    quint8 checksum = 0;
    for (int i = 0; i < 33; ++i) {
        checksum += static_cast<quint8>(data[i]);
    }
    if (checksum != static_cast<quint8>(data[33])) {
        qWarning() << "校验和错误";
        return flightData;
    }

    int index = 0;
    if (static_cast<quint8>(data[index]) != 0xEE || static_cast<quint8>(data[index+1]) != 0x81) {
        qWarning() << "无效的帧头或者消息ID";
        return flightData;
    }
    index+=2;
    if (static_cast<quint8>(data[index]) != 0x40 ) {
        qWarning() << QString("数据帧长字段有误,数据里的length为：%1").arg(data[index]);
        return flightData;
    }

    index++; // Skip frame length

    flightData.roll = static_cast<qint8>(data[index]) | (static_cast<qint8>(data[index+1]) << 8);
    index+=2;
    flightData.pitch = static_cast<qint8>(data[index]) | (static_cast<qint8>(data[index+1]) << 8);
    index+=2;
    flightData.heading = static_cast<qint8>(data[index]) | (static_cast<qint8>(data[index+1]) << 8);
    index+=2;
    flightData.latitude = static_cast<qint8>(data[index])
                          | (static_cast<qint8>(data[index+1]) << 8)
                          | (static_cast<qint8>(data[index+2]) << 16)
                          | (static_cast<qint8>(data[index+3]) << 24);
    index+=3;
    flightData.longitude = static_cast<qint8>(data[index])
                           | (static_cast<qint8>(data[index+1]) << 8)
                           | (static_cast<qint8>(data[index+2]) << 16)
                           | (static_cast<qint8>(data[index+3]) << 24);
    index+=3;
    flightData.gpsAltitude = static_cast<qint8>(data[index]) | (static_cast<qint8>(data[index+1]) << 8);
    index+=2;
    flightData.relativeAltitude = static_cast<qint8>(data[index]) | (static_cast<qint8>(data[index+1]) << 8);
    index+=2;
    flightData.year = static_cast<qint8>(data[index++]);
    flightData.month = static_cast<qint8>(data[index++]);
    flightData.day = static_cast<qint8>(data[index++]);
    flightData.hour = static_cast<qint8>(data[index++]);
    flightData.minute = static_cast<qint8>(data[index++]);
    flightData.second = static_cast<qint8>(data[index++]);
    flightData.eastSpeed = static_cast<qint8>(data[index]) | (static_cast<qint8>(data[index+1]) << 8);
    index+=2;
    flightData.northSpeed = static_cast<qint8>(data[index]) | (static_cast<qint8>(data[index+1]) << 8);
    index+=2;
    flightData.upSpeed = static_cast<qint8>(data[index]) | (static_cast<qint8>(data[index+1]) << 8);
    // index+=2;

    // Optional checksum verification can be added here

    return flightData;
}
